<html><!-- Created using the cpp_pretty_printer from the dlib C++ library.  See http://dlib.net for updates. --><head><title>dlib C++ Library - mpc.cpp</title></head><body bgcolor='white'><pre>
<font color='#009900'>// Copyright (C) 2015  Davis E. King (davis@dlib.net)
</font><font color='#009900'>// License: Boost Software License   See LICENSE.txt for the full license.
</font>

<font color='#0000FF'>#include</font> <font color='#5555FF'>&lt;</font>string<font color='#5555FF'>&gt;</font>
<font color='#0000FF'>#include</font> <font color='#5555FF'>&lt;</font>sstream<font color='#5555FF'>&gt;</font>

<font color='#0000FF'>#include</font> <font color='#5555FF'>&lt;</font>dlib<font color='#5555FF'>/</font>control.h<font color='#5555FF'>&gt;</font>
<font color='#0000FF'>#include</font> <font color='#5555FF'>&lt;</font>dlib<font color='#5555FF'>/</font>optimization.h<font color='#5555FF'>&gt;</font>
<font color='#0000FF'>#include</font> "<a style='text-decoration:none' href='tester.h.html'>tester.h</a>"

<font color='#0000FF'>namespace</font>  
<b>{</b>
    <font color='#0000FF'>using</font> <font color='#0000FF'>namespace</font> test;
    <font color='#0000FF'>using</font> <font color='#0000FF'>namespace</font> std;
    <font color='#0000FF'>using</font> <font color='#0000FF'>namespace</font> dlib;
  
    logger <b><a name='dlog'></a>dlog</b><font face='Lucida Console'>(</font>"<font color='#CC0000'>test.mpc</font>"<font face='Lucida Console'>)</font>;

    <font color='#0000FF'>template</font> <font color='#5555FF'>&lt;</font>
        <font color='#0000FF'>typename</font> EXP1,
        <font color='#0000FF'>typename</font> EXP2,
        <font color='#0000FF'>typename</font> T, <font color='#0000FF'><u>long</u></font> NR, <font color='#0000FF'><u>long</u></font> NC, <font color='#0000FF'>typename</font> MM, <font color='#0000FF'>typename</font> L
        <font color='#5555FF'>&gt;</font>
    <font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> <b><a name='solve_qp_box_using_smo'></a>solve_qp_box_using_smo</b> <font face='Lucida Console'>(</font> 
        <font color='#0000FF'>const</font> matrix_exp<font color='#5555FF'>&lt;</font>EXP1<font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> _Q,
        <font color='#0000FF'>const</font> matrix_exp<font color='#5555FF'>&lt;</font>EXP2<font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> _b,
        matrix<font color='#5555FF'>&lt;</font>T,NR,NC,MM,L<font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> alpha,
        matrix<font color='#5555FF'>&lt;</font>T,NR,NC,MM,L<font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> lower,
        matrix<font color='#5555FF'>&lt;</font>T,NR,NC,MM,L<font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> upper,
        T eps,
        <font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> max_iter
    <font face='Lucida Console'>)</font>
    <font color='#009900'>/*!
        ensures
            - solves: 0.5*trans(x)*Q*x + trans(b)*x where x is box constrained.
    !*/</font>
    <b>{</b>
        const_temp_matrix<font color='#5555FF'>&lt;</font>EXP1<font color='#5555FF'>&gt;</font> <font color='#BB00BB'>Q</font><font face='Lucida Console'>(</font>_Q<font face='Lucida Console'>)</font>;
        const_temp_matrix<font color='#5555FF'>&lt;</font>EXP2<font color='#5555FF'>&gt;</font> <font color='#BB00BB'>b</font><font face='Lucida Console'>(</font>_b<font face='Lucida Console'>)</font>;
        <font color='#009900'>//cout &lt;&lt; "IN QP SOLVER" &lt;&lt; endl;
</font>        <font color='#009900'>//cout &lt;&lt; "max eig: " &lt;&lt; max(real_eigenvalues(Q)) &lt;&lt; endl;
</font>        <font color='#009900'>//cout &lt;&lt; "min eig: " &lt;&lt; min(real_eigenvalues(Q)) &lt;&lt; endl;
</font>        <font color='#009900'>//cout &lt;&lt; "Q: \n" &lt;&lt; Q &lt;&lt; endl;
</font>        <font color='#009900'>//cout &lt;&lt; "b: \n" &lt;&lt; b &lt;&lt; endl;
</font>
        <font color='#009900'>// make sure requires clause is not broken
</font>        <font color='#BB00BB'>DLIB_ASSERT</font><font face='Lucida Console'>(</font>Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font><font color='#5555FF'>=</font> Q.<font color='#BB00BB'>nc</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     alpha.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font><font color='#5555FF'>=</font> lower.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     alpha.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font><font color='#5555FF'>=</font> upper.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     <font color='#BB00BB'>is_col_vector</font><font face='Lucida Console'>(</font>b<font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     <font color='#BB00BB'>is_col_vector</font><font face='Lucida Console'>(</font>alpha<font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     <font color='#BB00BB'>is_col_vector</font><font face='Lucida Console'>(</font>lower<font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     <font color='#BB00BB'>is_col_vector</font><font face='Lucida Console'>(</font>upper<font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     b.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font><font color='#5555FF'>=</font> alpha.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     b.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font><font color='#5555FF'>=</font> Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     alpha.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>&gt;</font> <font color='#979000'>0</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     <font color='#979000'>0</font> <font color='#5555FF'>&lt;</font><font color='#5555FF'>=</font> <font color='#BB00BB'>min</font><font face='Lucida Console'>(</font>alpha<font color='#5555FF'>-</font>lower<font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     <font color='#979000'>0</font> <font color='#5555FF'>&lt;</font><font color='#5555FF'>=</font> <font color='#BB00BB'>max</font><font face='Lucida Console'>(</font>upper<font color='#5555FF'>-</font>alpha<font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     eps <font color='#5555FF'>&gt;</font> <font color='#979000'>0</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font>
                     max_iter <font color='#5555FF'>&gt;</font> <font color='#979000'>0</font>,
                     "<font color='#CC0000'>\t unsigned long solve_qp_box_using_smo()</font>"
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t Invalid arguments were given to this function</font>"
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t Q.nr():               </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t Q.nc():               </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> Q.<font color='#BB00BB'>nc</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t is_col_vector(b):     </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>is_col_vector</font><font face='Lucida Console'>(</font>b<font face='Lucida Console'>)</font>
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t is_col_vector(alpha): </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>is_col_vector</font><font face='Lucida Console'>(</font>alpha<font face='Lucida Console'>)</font>
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t is_col_vector(lower): </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>is_col_vector</font><font face='Lucida Console'>(</font>lower<font face='Lucida Console'>)</font>
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t is_col_vector(upper): </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>is_col_vector</font><font face='Lucida Console'>(</font>upper<font face='Lucida Console'>)</font>
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t b.size():             </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> b.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> 
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t alpha.size():         </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> alpha.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> 
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t lower.size():         </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> lower.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> 
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t upper.size():         </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> upper.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> 
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t Q.nr():               </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> 
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t min(alpha-lower):     </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>min</font><font face='Lucida Console'>(</font>alpha<font color='#5555FF'>-</font>lower<font face='Lucida Console'>)</font> 
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t max(upper-alpha):     </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>max</font><font face='Lucida Console'>(</font>upper<font color='#5555FF'>-</font>alpha<font face='Lucida Console'>)</font> 
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t eps:                  </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> eps 
                     <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>\n\t max_iter:             </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> max_iter 
        <font face='Lucida Console'>)</font>;


        <font color='#009900'>// Compute f'(alpha) (i.e. the gradient of f(alpha)) for the current alpha.  
</font>        matrix<font color='#5555FF'>&lt;</font>T,NR,NC,MM,L<font color='#5555FF'>&gt;</font> df <font color='#5555FF'>=</font> Q<font color='#5555FF'>*</font>alpha <font color='#5555FF'>+</font> b;
        matrix<font color='#5555FF'>&lt;</font>T,NR,NC,MM,L<font color='#5555FF'>&gt;</font> QQ <font color='#5555FF'>=</font> <font color='#BB00BB'>reciprocal_max</font><font face='Lucida Console'>(</font><font color='#BB00BB'>diag</font><font face='Lucida Console'>(</font>Q<font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;


        <font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> iter <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
        <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font>; iter <font color='#5555FF'>&lt;</font> max_iter; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>iter<font face='Lucida Console'>)</font>
        <b>{</b>
            T max_df <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
            <font color='#0000FF'><u>long</u></font> best_r <font color='#5555FF'>=</font><font color='#979000'>0</font>;
            <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>long</u></font> r <font color='#5555FF'>=</font> <font color='#979000'>0</font>; r <font color='#5555FF'>&lt;</font> Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>r<font face='Lucida Console'>)</font>
            <b>{</b>
                <font color='#0000FF'>if</font> <font face='Lucida Console'>(</font><font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>&lt;</font><font color='#5555FF'>=</font> <font color='#BB00BB'>lower</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font> <font color='#BB00BB'>df</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>&gt;</font> <font color='#979000'>0</font><font face='Lucida Console'>)</font>
                    ;<font color='#009900'>//alpha(r) = lower(r);
</font>                <font color='#0000FF'>else</font> <font color='#0000FF'>if</font> <font face='Lucida Console'>(</font><font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>&gt;</font><font color='#5555FF'>=</font> <font color='#BB00BB'>upper</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font> <font color='#BB00BB'>df</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>&lt;</font> <font color='#979000'>0</font><font face='Lucida Console'>)</font>
                    ;<font color='#009900'>//alpha(r) = upper(r);
</font>                <font color='#0000FF'>else</font> <font color='#0000FF'>if</font> <font face='Lucida Console'>(</font>std::<font color='#BB00BB'>abs</font><font face='Lucida Console'>(</font><font color='#BB00BB'>df</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font><font face='Lucida Console'>)</font> <font color='#5555FF'>&gt;</font> max_df<font face='Lucida Console'>)</font>
                <b>{</b>
                    best_r <font color='#5555FF'>=</font> r;
                    max_df <font color='#5555FF'>=</font> std::<font color='#BB00BB'>abs</font><font face='Lucida Console'>(</font><font color='#BB00BB'>df</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;
                <b>}</b>
            <b>}</b>

            <font color='#009900'>//for (long r = 0; r &lt; Q.nr(); ++r)
</font>            <font color='#0000FF'><u>long</u></font> r <font color='#5555FF'>=</font> best_r;
            <b>{</b>

                <font color='#0000FF'>const</font> T old_alpha <font color='#5555FF'>=</font> <font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font>;
                <font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> <font color='#5555FF'>-</font><font face='Lucida Console'>(</font><font color='#BB00BB'>df</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font><font color='#5555FF'>-</font><font color='#BB00BB'>Q</font><font face='Lucida Console'>(</font>r,r<font face='Lucida Console'>)</font><font color='#5555FF'>*</font><font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font><font face='Lucida Console'>)</font><font color='#5555FF'>*</font><font color='#BB00BB'>QQ</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font>;
                <font color='#0000FF'>if</font> <font face='Lucida Console'>(</font><font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>&lt;</font> <font color='#BB00BB'>lower</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>
                    <font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> <font color='#BB00BB'>lower</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font>;
                <font color='#0000FF'>else</font> <font color='#0000FF'>if</font> <font face='Lucida Console'>(</font><font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>&gt;</font> <font color='#BB00BB'>upper</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>
                    <font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> <font color='#BB00BB'>upper</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font>;

                <font color='#0000FF'>const</font> T delta <font color='#5555FF'>=</font> old_alpha<font color='#5555FF'>-</font><font color='#BB00BB'>alpha</font><font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font>;

                <font color='#009900'>// Now update the gradient. We will perform the equivalent of: df = Q*alpha + b;
</font>                <font color='#0000FF'>for</font><font face='Lucida Console'>(</font><font color='#0000FF'><u>long</u></font> k <font color='#5555FF'>=</font> <font color='#979000'>0</font>; k <font color='#5555FF'>&lt;</font> df.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>k<font face='Lucida Console'>)</font>
                    <font color='#BB00BB'>df</font><font face='Lucida Console'>(</font>k<font face='Lucida Console'>)</font> <font color='#5555FF'>-</font><font color='#5555FF'>=</font> <font color='#BB00BB'>Q</font><font face='Lucida Console'>(</font>r,k<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>delta;
            <b>}</b>

            <font color='#0000FF'>if</font> <font face='Lucida Console'>(</font>max_df <font color='#5555FF'>&lt;</font> eps<font face='Lucida Console'>)</font>
                <font color='#0000FF'>break</font>;
        <b>}</b>
        <font color='#009900'>//cout &lt;&lt; "df: \n" &lt;&lt; trans(df) &lt;&lt; endl;
</font>        <font color='#009900'>//cout &lt;&lt; "objective value: " &lt;&lt; 0.5*trans(alpha)*Q*alpha + trans(b)*alpha &lt;&lt; endl;
</font>
        <font color='#0000FF'>return</font> iter<font color='#5555FF'>+</font><font color='#979000'>1</font>;
    <b>}</b>

<font color='#009900'>// ----------------------------------------------------------------------------------------
</font>
    <font color='#0000FF'>namespace</font> impl_mpc
    <b>{</b>
        <font color='#0000FF'>template</font> <font color='#5555FF'>&lt;</font><font color='#0000FF'><u>long</u></font> N<font color='#5555FF'>&gt;</font>
        <font color='#0000FF'><u>void</u></font> <b><a name='pack'></a>pack</b><font face='Lucida Console'>(</font>
            matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>0</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> out,
            <font color='#0000FF'>const</font> std::vector<font color='#5555FF'>&lt;</font>matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,N,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> <font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> item
        <font face='Lucida Console'>)</font>
        <b>{</b>
            <font color='#BB00BB'>DLIB_CASSERT</font><font face='Lucida Console'>(</font>item.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>!</font><font color='#5555FF'>=</font> <font color='#979000'>0</font>,"<font color='#CC0000'></font>"<font face='Lucida Console'>)</font>;
            out.<font color='#BB00BB'>set_size</font><font face='Lucida Console'>(</font>item.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font color='#5555FF'>*</font>item[<font color='#979000'>0</font>].<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;
            <font color='#0000FF'><u>long</u></font> j <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
            <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>0</font>; i <font color='#5555FF'>&lt;</font> item.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
                <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>long</u></font> r <font color='#5555FF'>=</font> <font color='#979000'>0</font>; r <font color='#5555FF'>&lt;</font> item[i].<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>r<font face='Lucida Console'>)</font>
                    <font color='#BB00BB'>out</font><font face='Lucida Console'>(</font>j<font color='#5555FF'>+</font><font color='#5555FF'>+</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> item[i]<font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font>;
        <b>}</b>

        <font color='#0000FF'>template</font> <font color='#5555FF'>&lt;</font><font color='#0000FF'><u>long</u></font> N<font color='#5555FF'>&gt;</font>
        <font color='#0000FF'><u>void</u></font> <b><a name='pack'></a>pack</b><font face='Lucida Console'>(</font>
            matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>0</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> out,
            <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,N,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> item,
            <font color='#0000FF'>const</font> <font color='#0000FF'><u>long</u></font> num
        <font face='Lucida Console'>)</font>
        <b>{</b>
            out.<font color='#BB00BB'>set_size</font><font face='Lucida Console'>(</font>item.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font color='#5555FF'>*</font>num<font face='Lucida Console'>)</font>;
            <font color='#0000FF'><u>long</u></font> j <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
            <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>long</u></font> r <font color='#5555FF'>=</font> <font color='#979000'>0</font>; r <font color='#5555FF'>&lt;</font> num; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>r<font face='Lucida Console'>)</font>
                <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>long</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>0</font>; i <font color='#5555FF'>&lt;</font> item.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
                    <font color='#BB00BB'>out</font><font face='Lucida Console'>(</font>j<font color='#5555FF'>+</font><font color='#5555FF'>+</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> <font color='#BB00BB'>item</font><font face='Lucida Console'>(</font>i<font face='Lucida Console'>)</font>;
        <b>}</b>

        <font color='#0000FF'>template</font> <font color='#5555FF'>&lt;</font><font color='#0000FF'><u>long</u></font> N<font color='#5555FF'>&gt;</font>
        <font color='#0000FF'><u>void</u></font> <b><a name='unpack'></a>unpack</b><font face='Lucida Console'>(</font>
            std::vector<font color='#5555FF'>&lt;</font>matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,N,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> <font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> out,
            <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>0</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> item 
        <font face='Lucida Console'>)</font>
        <b>{</b>
            <font color='#BB00BB'>DLIB_CASSERT</font><font face='Lucida Console'>(</font>out.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>!</font><font color='#5555FF'>=</font> <font color='#979000'>0</font>,"<font color='#CC0000'></font>"<font face='Lucida Console'>)</font>;
            <font color='#BB00BB'>DLIB_CASSERT</font><font face='Lucida Console'>(</font><font face='Lucida Console'>(</font><font color='#0000FF'><u>long</u></font><font face='Lucida Console'>)</font>out.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font color='#5555FF'>*</font>out[<font color='#979000'>0</font>].<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font><font color='#5555FF'>=</font> item.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,"<font color='#CC0000'></font>"<font face='Lucida Console'>)</font>;
            <font color='#0000FF'><u>long</u></font> j <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
            <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>0</font>; i <font color='#5555FF'>&lt;</font> out.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
                <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>long</u></font> r <font color='#5555FF'>=</font> <font color='#979000'>0</font>; r <font color='#5555FF'>&lt;</font> out[i].<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>r<font face='Lucida Console'>)</font>
                    out[i]<font face='Lucida Console'>(</font>r<font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> <font color='#BB00BB'>item</font><font face='Lucida Console'>(</font>j<font color='#5555FF'>+</font><font color='#5555FF'>+</font><font face='Lucida Console'>)</font>;
        <b>}</b>
    <b>}</b>

    <font color='#0000FF'>template</font> <font color='#5555FF'>&lt;</font><font color='#0000FF'><u>long</u></font> S, <font color='#0000FF'><u>long</u></font> I<font color='#5555FF'>&gt;</font>
    <font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> <b><a name='solve_linear_mpc'></a>solve_linear_mpc</b> <font face='Lucida Console'>(</font>
        <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,S,S<font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> A,
        <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,S,I<font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> B,
        <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,S,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> C,
        <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,S,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> Q,
        <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,I,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> R,
        <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,I,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> _lower,
        <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,I,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> _upper,
        <font color='#0000FF'>const</font> std::vector<font color='#5555FF'>&lt;</font>matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,S,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> <font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> target, 
        <font color='#0000FF'>const</font> matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,S,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> initial_state,
        std::vector<font color='#5555FF'>&lt;</font>matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,I,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> <font color='#5555FF'>&gt;</font><font color='#5555FF'>&amp;</font> controls <font color='#009900'>// input and output
</font>    <font face='Lucida Console'>)</font>
    <b>{</b>
        <font color='#0000FF'>using</font> <font color='#0000FF'>namespace</font> impl_mpc;
        <font color='#BB00BB'>DLIB_CASSERT</font><font face='Lucida Console'>(</font>target.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font><font color='#5555FF'>=</font> controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,"<font color='#CC0000'></font>"<font face='Lucida Console'>)</font>;

        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font><font color='#5555FF'>&gt;</font> <font color='#BB00BB'>K</font><font face='Lucida Console'>(</font>B.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font color='#5555FF'>*</font>controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>, B.<font color='#BB00BB'>nc</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font color='#5555FF'>*</font>controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;
        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>0</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> <font color='#BB00BB'>M</font><font face='Lucida Console'>(</font>B.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font color='#5555FF'>*</font>controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;

        <font color='#009900'>// compute powers of A: Apow[i] == A^i
</font>        std::vector<font color='#5555FF'>&lt;</font>matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,S,S<font color='#5555FF'>&gt;</font> <font color='#5555FF'>&gt;</font> <font color='#BB00BB'>Apow</font><font face='Lucida Console'>(</font>controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;
        Apow[<font color='#979000'>0</font>] <font color='#5555FF'>=</font> <font color='#BB00BB'>identity_matrix</font><font face='Lucida Console'>(</font>A<font face='Lucida Console'>)</font>;
        <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>1</font>; i <font color='#5555FF'>&lt;</font> Apow.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
            Apow[i] <font color='#5555FF'>=</font> A<font color='#5555FF'>*</font>Apow[i<font color='#5555FF'>-</font><font color='#979000'>1</font>];

        <font color='#009900'>// fill in K
</font>        K <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
        <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> r <font color='#5555FF'>=</font> <font color='#979000'>0</font>; r <font color='#5555FF'>&lt;</font> controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>r<font face='Lucida Console'>)</font>
            <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> c <font color='#5555FF'>=</font> <font color='#979000'>0</font>; c <font color='#5555FF'>&lt;</font><font color='#5555FF'>=</font> r; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>c<font face='Lucida Console'>)</font>
                <font color='#BB00BB'>set_subm</font><font face='Lucida Console'>(</font>K,r<font color='#5555FF'>*</font>B.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,c<font color='#5555FF'>*</font>B.<font color='#BB00BB'>nc</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>, B.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>, B.<font color='#BB00BB'>nc</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> Apow[r<font color='#5555FF'>-</font>c]<font color='#5555FF'>*</font>B;

        <font color='#009900'>// fill in M
</font>        <font color='#BB00BB'>set_subm</font><font face='Lucida Console'>(</font>M,<font color='#979000'>0</font><font color='#5555FF'>*</font>A.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,<font color='#979000'>0</font>,A.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,<font color='#979000'>1</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> A<font color='#5555FF'>*</font>initial_state <font color='#5555FF'>+</font> C;
        <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>1</font>; i <font color='#5555FF'>&lt;</font> controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
            <font color='#BB00BB'>set_subm</font><font face='Lucida Console'>(</font>M,i<font color='#5555FF'>*</font>A.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,<font color='#979000'>0</font>,A.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,<font color='#979000'>1</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> A<font color='#5555FF'>*</font><font color='#BB00BB'>subm</font><font face='Lucida Console'>(</font>M,<font face='Lucida Console'>(</font>i<font color='#5555FF'>-</font><font color='#979000'>1</font><font face='Lucida Console'>)</font><font color='#5555FF'>*</font>A.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,<font color='#979000'>0</font>,A.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,<font color='#979000'>1</font><font face='Lucida Console'>)</font> <font color='#5555FF'>+</font> C;

        <font color='#009900'>//cout &lt;&lt; "M: \n" &lt;&lt; M &lt;&lt; endl;
</font>        <font color='#009900'>//cout &lt;&lt; "K: \n" &lt;&lt; K &lt;&lt; endl;
</font>
        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>0</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> t, v, lower, upper;
        <font color='#BB00BB'>pack</font><font face='Lucida Console'>(</font>t, target<font face='Lucida Console'>)</font>;
        <font color='#BB00BB'>pack</font><font face='Lucida Console'>(</font>v, controls<font face='Lucida Console'>)</font>;
        <font color='#BB00BB'>pack</font><font face='Lucida Console'>(</font>lower, _lower, controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;
        <font color='#BB00BB'>pack</font><font face='Lucida Console'>(</font>upper, _upper, controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;


        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font><font color='#5555FF'>&gt;</font> <font color='#BB00BB'>QQ</font><font face='Lucida Console'>(</font>K.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,K.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>, <font color='#BB00BB'>RR</font><font face='Lucida Console'>(</font>K.<font color='#BB00BB'>nc</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,K.<font color='#BB00BB'>nc</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;
        QQ <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
        RR <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
        <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> c <font color='#5555FF'>=</font> <font color='#979000'>0</font>; c <font color='#5555FF'>&lt;</font> controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>c<font face='Lucida Console'>)</font>
        <b>{</b>
            <font color='#BB00BB'>set_subm</font><font face='Lucida Console'>(</font>QQ,c<font color='#5555FF'>*</font>Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,c<font color='#5555FF'>*</font>Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,Q.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> <font color='#BB00BB'>diagm</font><font face='Lucida Console'>(</font>Q<font face='Lucida Console'>)</font>;
            <font color='#BB00BB'>set_subm</font><font face='Lucida Console'>(</font>RR,c<font color='#5555FF'>*</font>R.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,c<font color='#5555FF'>*</font>R.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,R.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>,R.<font color='#BB00BB'>nr</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> <font color='#BB00BB'>diagm</font><font face='Lucida Console'>(</font>R<font face='Lucida Console'>)</font>;
        <b>}</b>

        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font><font color='#5555FF'>&gt;</font> m1 <font color='#5555FF'>=</font> <font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>K<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>QQ<font color='#5555FF'>*</font>K<font color='#5555FF'>+</font>RR;
        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font><font color='#5555FF'>&gt;</font> m2 <font color='#5555FF'>=</font> <font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>K<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>QQ<font color='#5555FF'>*</font><font face='Lucida Console'>(</font>M<font color='#5555FF'>-</font>t<font face='Lucida Console'>)</font>;


        <font color='#009900'>// run the solver...
</font>        <font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> iter;
        iter <font color='#5555FF'>=</font> <font color='#BB00BB'>solve_qp_box_using_smo</font><font face='Lucida Console'>(</font>
            m1,
            m2,
            v,
            lower,
            upper,
            <font color='#979000'>0.00000001</font>,
            <font color='#979000'>100000</font><font face='Lucida Console'>)</font>;

        <font color='#009900'>//cout &lt;&lt; "iterations: " &lt;&lt; iter &lt;&lt; endl;
</font>
        <font color='#BB00BB'>unpack</font><font face='Lucida Console'>(</font>controls, v<font face='Lucida Console'>)</font>;
        <font color='#0000FF'>return</font> iter;
    <b>}</b>

    <font color='#0000FF'><u>void</u></font> <b><a name='test_with_positive_target_error_thresh'></a>test_with_positive_target_error_thresh</b><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font> 
    <b>{</b>
        <font color='#009900'>// a basic position + velocity model
</font>        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>2</font><font color='#5555FF'>&gt;</font> A;
        A <font color='#5555FF'>=</font> <font color='#979000'>1</font>, <font color='#979000'>1</font>,
            <font color='#979000'>0</font>, <font color='#979000'>1</font>;
        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> B, C;
        B <font color='#5555FF'>=</font> <font color='#979000'>0</font>,
            <font color='#979000'>1</font>;

        C <font color='#5555FF'>=</font> <font color='#979000'>0.0</font>,<font color='#979000'>0.0</font>; <font color='#009900'>// no constant bias
</font>
        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> Q;
        Q <font color='#5555FF'>=</font> <font color='#979000'>2</font>, <font color='#979000'>0</font>; <font color='#009900'>// only care about getting the position right
</font>        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>1</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> R, lower, upper;
        R <font color='#5555FF'>=</font> <font color='#979000'>0.001</font>;

        <font color='#009900'>// We set it up so that the controller that only cares about getting to the target but
</font>        <font color='#009900'>// doesn't care about what happens after can just give large controls of -1.  It will fly
</font>        <font color='#009900'>// past the target once it gets there, but it doesn't care about that.  The solver_normal
</font>        <font color='#009900'>// however wants to come to rest at the target.  So it will have to give a much smaller
</font>        <font color='#009900'>// control to get it moving towards the target and then slowly decelerate over time to end
</font>        <font color='#009900'>// up stopping.  So it will take longer to get to the target, which we test below.
</font>        lower <font color='#5555FF'>=</font> <font color='#5555FF'>-</font><font color='#979000'>1.0</font>;
        upper <font color='#5555FF'>=</font>  <font color='#979000'>0.05</font>;

        mpc<font color='#5555FF'>&lt;</font><font color='#979000'>2</font>,<font color='#979000'>1</font>,<font color='#979000'>30</font><font color='#5555FF'>&gt;</font> <font color='#BB00BB'>solver_normal</font><font face='Lucida Console'>(</font>A,B,C,Q,R,lower,upper<font face='Lucida Console'>)</font>;
        solver_normal.<font color='#BB00BB'>set_epsilon</font><font face='Lucida Console'>(</font><font color='#979000'>0.00000001</font><font face='Lucida Console'>)</font>;
        solver_normal.<font color='#BB00BB'>set_max_iterations</font><font face='Lucida Console'>(</font><font color='#979000'>10000</font><font face='Lucida Console'>)</font>;

        mpc<font color='#5555FF'>&lt;</font><font color='#979000'>2</font>,<font color='#979000'>1</font>,<font color='#979000'>30</font><font color='#5555FF'>&gt;</font> solver_target_thresh <font color='#5555FF'>=</font> solver_normal;
        solver_target_thresh.<font color='#BB00BB'>set_target_error_threshold</font><font face='Lucida Console'>(</font><font color='#979000'>1.0</font><font face='Lucida Console'>)</font>;

        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> state_normal;
        state_normal <font color='#5555FF'>=</font> <font color='#979000'>10</font>, <font color='#979000'>0</font>;
        matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> state_target_thresh <font color='#5555FF'>=</font> state_normal;

        <font color='#009900'>// Run the control law with and without set_target_error_threshold() called.  We should
</font>        <font color='#009900'>// observe that the controller using set_target_error_threshold(1) drives us to the target
</font>        <font color='#009900'>// state faster, however, only the normal solver results in a rest state at the target
</font>        <font color='#009900'>// state.
</font>        <font color='#0000FF'><u>int</u></font> time_at_target_normal <font color='#5555FF'>=</font> <font color='#5555FF'>-</font><font color='#979000'>1</font>;
        <font color='#0000FF'><u>int</u></font> time_at_target_other  <font color='#5555FF'>=</font> <font color='#5555FF'>-</font><font color='#979000'>1</font>;
        <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>int</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>0</font>; i <font color='#5555FF'>&lt;</font> <font color='#979000'>30</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
        <b>{</b>
            <font color='#BB00BB'>print_spinner</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>;
            <font color='#0000FF'>const</font> <font color='#0000FF'><u>double</u></font> normal_control <font color='#5555FF'>=</font> <font color='#BB00BB'>solver_normal</font><font face='Lucida Console'>(</font>state_normal<font face='Lucida Console'>)</font>;
            state_normal <font color='#5555FF'>=</font> A<font color='#5555FF'>*</font>state_normal <font color='#5555FF'>+</font> B<font color='#5555FF'>*</font>normal_control <font color='#5555FF'>+</font> C;
            
            <font color='#0000FF'>const</font> <font color='#0000FF'><u>double</u></font> target_thresh_control <font color='#5555FF'>=</font> <font color='#BB00BB'>solver_target_thresh</font><font face='Lucida Console'>(</font>state_target_thresh<font face='Lucida Console'>)</font>;
            state_target_thresh <font color='#5555FF'>=</font> A<font color='#5555FF'>*</font>state_target_thresh <font color='#5555FF'>+</font> B<font color='#5555FF'>*</font>target_thresh_control <font color='#5555FF'>+</font> C;

            <font color='#0000FF'>const</font> <font color='#0000FF'><u>double</u></font> normal_error <font color='#5555FF'>=</font> <font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>state_normal<font face='Lucida Console'>)</font><font color='#5555FF'>*</font><font color='#BB00BB'>diagm</font><font face='Lucida Console'>(</font>Q<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>state_normal;
            <font color='#0000FF'>const</font> <font color='#0000FF'><u>double</u></font> target_error <font color='#5555FF'>=</font> <font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>state_target_thresh<font face='Lucida Console'>)</font><font color='#5555FF'>*</font><font color='#BB00BB'>diagm</font><font face='Lucida Console'>(</font>Q<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>state_target_thresh;
            <font color='#0000FF'>if</font> <font face='Lucida Console'>(</font>normal_error <font color='#5555FF'>&lt;</font> <font color='#979000'>1.0</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font> time_at_target_normal <font color='#5555FF'>=</font><font color='#5555FF'>=</font> <font color='#5555FF'>-</font><font color='#979000'>1</font><font face='Lucida Console'>)</font>
                time_at_target_normal <font color='#5555FF'>=</font> i;
            <font color='#0000FF'>if</font> <font face='Lucida Console'>(</font>target_error <font color='#5555FF'>&lt;</font> <font color='#979000'>1.0</font> <font color='#5555FF'>&amp;</font><font color='#5555FF'>&amp;</font> time_at_target_other <font color='#5555FF'>=</font><font color='#5555FF'>=</font> <font color='#5555FF'>-</font><font color='#979000'>1</font><font face='Lucida Console'>)</font>
                time_at_target_other <font color='#5555FF'>=</font> i;
        <b>}</b>

        <font color='#009900'>// Check that the normal solver took longer to get to the target.
</font>        <font color='#BB00BB'>DLIB_TEST</font><font face='Lucida Console'>(</font>time_at_target_normal <font color='#5555FF'>=</font><font color='#5555FF'>=</font> <font color='#979000'>13</font><font face='Lucida Console'>)</font>;
        <font color='#BB00BB'>DLIB_TEST</font><font face='Lucida Console'>(</font>time_at_target_other <font color='#5555FF'>=</font><font color='#5555FF'>=</font> <font color='#979000'>7</font><font face='Lucida Console'>)</font>;

        <font color='#009900'>// Check that the normal solver ends up right on the target.
</font>        <font color='#BB00BB'>DLIB_TEST</font><font face='Lucida Console'>(</font><font color='#BB00BB'>length</font><font face='Lucida Console'>(</font>state_normal<font face='Lucida Console'>)</font> <font color='#5555FF'>&lt;</font> <font color='#979000'>0.01</font><font face='Lucida Console'>)</font>;
        <font color='#009900'>// But the one that got there faster blew past the target and is no way off in the distance.
</font>        <font color='#BB00BB'>DLIB_TEST</font><font face='Lucida Console'>(</font><font color='#BB00BB'>length</font><font face='Lucida Console'>(</font>state_target_thresh<font face='Lucida Console'>)</font> <font color='#5555FF'>&gt;</font> <font color='#979000'>20</font><font face='Lucida Console'>)</font>;
    <b>}</b>


    <font color='#0000FF'>class</font> <b><a name='test_mpc'></a>test_mpc</b> : <font color='#0000FF'>public</font> tester
    <b>{</b>
    <font color='#0000FF'>public</font>:
        <b><a name='test_mpc'></a>test_mpc</b> <font face='Lucida Console'>(</font>
        <font face='Lucida Console'>)</font> :
            tester <font face='Lucida Console'>(</font>"<font color='#CC0000'>test_mpc</font>",
                    "<font color='#CC0000'>Runs tests on the mpc object.</font>"<font face='Lucida Console'>)</font>
        <b>{</b><b>}</b>

        <font color='#0000FF'><u>void</u></font> <b><a name='perform_test'></a>perform_test</b> <font face='Lucida Console'>(</font>
        <font face='Lucida Console'>)</font>
        <b>{</b>
            <font color='#009900'>// a basic position + velocity model
</font>            matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>2</font><font color='#5555FF'>&gt;</font> A;
            A <font color='#5555FF'>=</font> <font color='#979000'>1</font>, <font color='#979000'>1</font>,
            <font color='#979000'>0</font>, <font color='#979000'>1</font>;
            matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> B, C;
            B <font color='#5555FF'>=</font> <font color='#979000'>0</font>,
            <font color='#979000'>1</font>;

            C <font color='#5555FF'>=</font> <font color='#979000'>0.02</font>,<font color='#979000'>0.1</font>; <font color='#009900'>// no constant bias
</font>
            matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> Q;
            Q <font color='#5555FF'>=</font> <font color='#979000'>2</font>, <font color='#979000'>0</font>; <font color='#009900'>// only care about getting the position right
</font>            matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>1</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> R, lower, upper;
            R <font color='#5555FF'>=</font> <font color='#979000'>1</font>;

            lower <font color='#5555FF'>=</font> <font color='#5555FF'>-</font><font color='#979000'>0.2</font>;
            upper <font color='#5555FF'>=</font>  <font color='#979000'>0.2</font>;

            std::vector<font color='#5555FF'>&lt;</font>matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>1</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> <font color='#5555FF'>&gt;</font> <font color='#BB00BB'>controls</font><font face='Lucida Console'>(</font><font color='#979000'>30</font><font face='Lucida Console'>)</font>;
            std::vector<font color='#5555FF'>&lt;</font>matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> <font color='#5555FF'>&gt;</font> <font color='#BB00BB'>target</font><font face='Lucida Console'>(</font><font color='#979000'>30</font><font face='Lucida Console'>)</font>;
            <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>0</font>; i <font color='#5555FF'>&lt;</font> controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
            <b>{</b>
                controls[i] <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
                target[i] <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
            <b>}</b>

            mpc<font color='#5555FF'>&lt;</font><font color='#979000'>2</font>,<font color='#979000'>1</font>,<font color='#979000'>30</font><font color='#5555FF'>&gt;</font> <font color='#BB00BB'>solver</font><font face='Lucida Console'>(</font>A,B,C,Q,R,lower,upper<font face='Lucida Console'>)</font>;
            solver.<font color='#BB00BB'>set_epsilon</font><font face='Lucida Console'>(</font><font color='#979000'>0.00000001</font><font face='Lucida Console'>)</font>;
            solver.<font color='#BB00BB'>set_max_iterations</font><font face='Lucida Console'>(</font><font color='#979000'>10000</font><font face='Lucida Console'>)</font>;
            matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>2</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> initial_state;
            initial_state <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
            <font color='#BB00BB'>initial_state</font><font face='Lucida Console'>(</font><font color='#979000'>0</font><font face='Lucida Console'>)</font> <font color='#5555FF'>=</font> <font color='#979000'>5</font>;
            <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>int</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>0</font>; i <font color='#5555FF'>&lt;</font> <font color='#979000'>30</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
            <b>{</b>
                <font color='#BB00BB'>print_spinner</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>;
                matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>1</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> control <font color='#5555FF'>=</font> <font color='#BB00BB'>solver</font><font face='Lucida Console'>(</font>initial_state<font face='Lucida Console'>)</font>;

                <font color='#0000FF'>for</font> <font face='Lucida Console'>(</font><font color='#0000FF'><u>unsigned</u></font> <font color='#0000FF'><u>long</u></font> i <font color='#5555FF'>=</font> <font color='#979000'>1</font>; i <font color='#5555FF'>&lt;</font> controls.<font color='#BB00BB'>size</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>; <font color='#5555FF'>+</font><font color='#5555FF'>+</font>i<font face='Lucida Console'>)</font>
                    controls[i<font color='#5555FF'>-</font><font color='#979000'>1</font>] <font color='#5555FF'>=</font> controls[i];

                <font color='#009900'>// Compute the correct control via SMO and make sure it matches.
</font>                <font color='#BB00BB'>solve_linear_mpc</font><font face='Lucida Console'>(</font>A,B,C,Q,R,lower,upper, target, initial_state, controls<font face='Lucida Console'>)</font>;
                dlog <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> LINFO <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>ERROR: </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>length</font><font face='Lucida Console'>(</font>control<font color='#5555FF'>-</font>controls[<font color='#979000'>0</font>]<font face='Lucida Console'>)</font>;
                <font color='#BB00BB'>DLIB_TEST</font><font face='Lucida Console'>(</font><font color='#BB00BB'>length</font><font face='Lucida Console'>(</font>control<font color='#5555FF'>-</font>controls[<font color='#979000'>0</font>]<font face='Lucida Console'>)</font> <font color='#5555FF'>&lt;</font> <font color='#979000'>1e</font><font color='#5555FF'>-</font><font color='#979000'>7</font><font face='Lucida Console'>)</font>;

                initial_state <font color='#5555FF'>=</font> A<font color='#5555FF'>*</font>initial_state <font color='#5555FF'>+</font> B<font color='#5555FF'>*</font>control <font color='#5555FF'>+</font> C;
                <font color='#009900'>//cout &lt;&lt; control(0) &lt;&lt; "\t" &lt;&lt; trans(initial_state);
</font>            <b>}</b>

            <b>{</b>
                <font color='#009900'>// also just generally test our QP solver.
</font>                matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>20</font>,<font color='#979000'>20</font><font color='#5555FF'>&gt;</font> Q <font color='#5555FF'>=</font> <font color='#BB00BB'>gaussian_randm</font><font face='Lucida Console'>(</font><font color='#979000'>20</font>,<font color='#979000'>20</font>,<font color='#979000'>5</font><font face='Lucida Console'>)</font>;
                Q <font color='#5555FF'>=</font> Q<font color='#5555FF'>*</font><font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>Q<font face='Lucida Console'>)</font>;

                matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>20</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> b <font color='#5555FF'>=</font> <font color='#BB00BB'>randm</font><font face='Lucida Console'>(</font><font color='#979000'>20</font>,<font color='#979000'>1</font><font face='Lucida Console'>)</font><font color='#5555FF'>-</font><font color='#979000'>0.5</font>;
                matrix<font color='#5555FF'>&lt;</font><font color='#0000FF'><u>double</u></font>,<font color='#979000'>20</font>,<font color='#979000'>1</font><font color='#5555FF'>&gt;</font> alpha, lower, upper, alpha2;
                alpha <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
                alpha2 <font color='#5555FF'>=</font> <font color='#979000'>0</font>;
                lower <font color='#5555FF'>=</font> <font color='#5555FF'>-</font><font color='#979000'>4</font>;
                upper <font color='#5555FF'>=</font> <font color='#979000'>3</font>;

                <font color='#BB00BB'>solve_qp_box_using_smo</font><font face='Lucida Console'>(</font>Q,b,alpha,lower, upper, <font color='#979000'>1e</font><font color='#5555FF'>-</font><font color='#979000'>12</font>, <font color='#979000'>500000</font><font face='Lucida Console'>)</font>;
                <font color='#BB00BB'>solve_qp_box_constrained</font><font face='Lucida Console'>(</font>Q,b,alpha2,lower, upper, <font color='#979000'>1e</font><font color='#5555FF'>-</font><font color='#979000'>12</font>, <font color='#979000'>50000</font><font face='Lucida Console'>)</font>;
                dlog <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> LINFO <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>alpha<font face='Lucida Console'>)</font>;
                dlog <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> LINFO <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>alpha2<font face='Lucida Console'>)</font>;
                dlog <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> LINFO <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>objective value:  </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#979000'>0.5</font><font color='#5555FF'>*</font><font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>alpha<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>Q<font color='#5555FF'>*</font>alpha <font color='#5555FF'>+</font> <font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>b<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>alpha;
                dlog <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> LINFO <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> "<font color='#CC0000'>objective value2: </font>" <font color='#5555FF'>&lt;</font><font color='#5555FF'>&lt;</font> <font color='#979000'>0.5</font><font color='#5555FF'>*</font><font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>alpha2<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>Q<font color='#5555FF'>*</font>alpha <font color='#5555FF'>+</font> <font color='#BB00BB'>trans</font><font face='Lucida Console'>(</font>b<font face='Lucida Console'>)</font><font color='#5555FF'>*</font>alpha2;
                <font color='#BB00BB'>DLIB_TEST_MSG</font><font face='Lucida Console'>(</font><font color='#BB00BB'>max</font><font face='Lucida Console'>(</font><font color='#BB00BB'>abs</font><font face='Lucida Console'>(</font>alpha<font color='#5555FF'>-</font>alpha2<font face='Lucida Console'>)</font><font face='Lucida Console'>)</font> <font color='#5555FF'>&lt;</font> <font color='#979000'>1e</font><font color='#5555FF'>-</font><font color='#979000'>7</font>, <font color='#BB00BB'>max</font><font face='Lucida Console'>(</font><font color='#BB00BB'>abs</font><font face='Lucida Console'>(</font>alpha<font color='#5555FF'>-</font>alpha2<font face='Lucida Console'>)</font><font face='Lucida Console'>)</font><font face='Lucida Console'>)</font>;
            <b>}</b>

            <font color='#BB00BB'>test_with_positive_target_error_thresh</font><font face='Lucida Console'>(</font><font face='Lucida Console'>)</font>;
        <b>}</b>
    <b>}</b> a;

<b>}</b>





</pre></body></html>